﻿#pragma once
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense> 
#include <Eigen/Core>
#include <Eigen/Geometry>// Eigen 几何模块
#include<vector>

class AlgorithmVision3D
{
public:
    AlgorithmVision3D();

public:
	//创建一个Eigen::Matrix4f矩阵
	Eigen::Matrix4f CreateMatrix4f(
		const float& fm00, const float& fm01, const float& fm02, const float& fm03,
		const float& fm10, const float& fm11, const float& fm12, const float& fm13,
		const float& fm20, const float& fm21, const float& fm22, const float& fm23,
		const float& fm30 = 0, const float& fm31 = 0, const float& fm32 = 0, const float& fm33 = 1);

	//创建一个Eigen::Matrix4d矩阵
	Eigen::Matrix4d CreateMatrix4d(
		const float& dm00, const float& dm01, const float& dm02, const float& dm03,
		const float& dm10, const float& dm11, const float& dm12, const float& dm13,
		const float& dm20, const float& dm21, const float& dm22, const float& dm23,
		const float& dm30 = 0, const float& dm31 = 0, const float& dm32 = 0, const float& dm33 = 1);

	//创建一个Eigen::Matrix3f矩阵
	Eigen::Matrix3f CreateMatrix3f(
		const float& fm00, const float& fm01, const float& fm02,
		const float& fm10, const float& fm11, const float& fm12,
		const float& fm20 = 0, const float& fm21 = 0, const float& fm22 = 1);

	//创建一个Eigen::Matrix3d矩阵
	Eigen::Matrix3d CreateMatrix3d(
		const float& fm00, const float& fm01, const float& fm02,
		const float& fm10, const float& fm11, const float& fm12,
		const float& fm20 = 0, const float& fm21 = 0, const float& fm22 = 1);

public:
	inline Eigen::Matrix3d Matrix3f2Matrix3d(const Eigen::Matrix3f& Matrix3fR);

	inline Eigen::Matrix3f Matrix3d2Matrix3f(const Eigen::Matrix3d& Matrix3dR);

	inline Eigen::Affine3d Matrix4d2Affine3d(const Eigen::Matrix4d& matrix4d);

	inline Eigen::Affine3f Matrix4f2Affine3f(const Eigen::Matrix4f& matrix4f);

	//创建一个Matrix3d转Eigen::Matrix4d矩阵
	Eigen::Matrix4d Matrix3d2Matrix4d(const Eigen::Matrix3d& Matrix3dR, const double& dTx = 0, const double& dTy = 0, const double& dTz = 0);
	Eigen::Matrix4f Matrix3f2Matrix4f(const Eigen::Matrix3f& Matrix3fR, const float& fTx = 0, const float& fTy = 0, const float& fTz = 0);

	//Eigen::Matrix4d矩阵转为Matrix3d与xyz平移量
	Eigen::Matrix3d Matrix4d2Matrix3d(const Eigen::Matrix4d& Matrix4dR, double& dTx, double& dTy, double& dTz);
	Eigen::Matrix3d Matrix4d2Matrix3d(const Eigen::Matrix4d& Matrix4dR);

	Eigen::Matrix3f Matrix4f2Matrix3f(const Eigen::Matrix4f& Matrix4fR);

	//显示Matrix4f位姿矩阵
	void ShowMatrix4f(const Eigen::Matrix4f& Pose, const std::string& sNameLine = "");
	//显示Matrix4d矩阵
	void ShowMatrix4d(const Eigen::Matrix4d& Pose, const std::string& sNameLine = "");
	//转俯仰角形式显示Matrix4f矩阵
	void ShowMatrix4fonRPY(const Eigen::Matrix4f& Pose, const std::string& sNameLine = "");
	//转俯仰角形式显示Matrix4f矩阵
	void ShowMatrix4donRPY(const Eigen::Matrix4d& Pose, const std::string& sNameLine = "");

	//显示Mat4d位姿矩阵
	void ShowHomMat3D(const cv::Mat& matMatrix4d, const std::string& sNameLine = "");
	//转俯仰角形式显示Mat4d位姿矩阵
	//ShowHomMat3DonRPY(HomMat3DCamInTCP);
	//ShowHomMat3DonRPY(HomMat3DCamInTCP, "矩阵HomMat3DCamInTCP");
	void ShowHomMat3DonRPY(const cv::Mat& matMatrix4d, const std::string& sNameLine = "");

	//Matrix4f 转 opencv mat
	cv::Mat Matrix4f2Mat4d(Eigen::Matrix4f matMatrix4f);
	//Matrix4d 转 opencv mat
	cv::Mat Matrix4d2Mat4d(Eigen::Matrix4d matMatrix4d);

	//opencv mat转Matrix4f
	Eigen::Matrix4f Mat4d2Matrix4f(cv::Mat matMatrix4d);
	//opencv mat转Matrix4d
	Eigen::Matrix4d Mat4d2Matrix4d(cv::Mat matMatrix4d);

	//vvd转Mat矩阵，依据vvd矩阵大小创建mat
	cv::Mat vvd2Matd(const std::vector< std::vector<double>>& vvdTable);
	cv::Mat vvd2Matf(const std::vector< std::vector<double>>& vvdTable);

	Eigen::Matrix4f vvd2Matrix4f(const std::vector< std::vector<double>>& vvdTable);
	Eigen::Matrix4d vvd2Matrix4d(const std::vector< std::vector<double>>& vvdTable);


	std::vector< std::vector<double>> Matrix4f2vvd(const Eigen::Matrix4f matrix4f);
	std::vector< std::vector<double>> Matrix4d2vvd(const Eigen::Matrix4d matrix4d);


	//cvCloud2vPoint3f 有序点云转无序点云 0 0 0 点云为无效点云 仅转换一个 先set后转vector   改为不保存
	std::vector<cv::Point3f> cvCloud2vPoint3f(const cv::Mat& cvCloud, bool bswapXY = false);

	//vvd转vPoint
	std::vector<cv::Point3f> vvd2vPoint3f(const std::vector< std::vector<double>>& vvdTable);
	std::vector<cv::Point3d> vvd2vPoint3d(const std::vector< std::vector<double>>& vvdTable);

	//vPoint转vvd
	std::vector< std::vector<double>> vPoint2vvd(const std::vector<cv::Point2f>& vPoint);
	std::vector< std::vector<double>> vPoint2vvd(const std::vector<cv::Point3f>& vPoint);

	//Mat矩阵转vvd表
	//std::vector< std::vector<double>> vvdHomMat4d = mat2vvd(HomMat4d);
	std::vector< std::vector<double>> mat2vvd(const cv::Mat& dataTable);

	//3D点表转mat齐次矩阵表，多一列1
	cv::Mat vvd3DPoint2MatfHom(const std::vector< std::vector<double>>& vvd3DPoint);

	//vvd表转置
	std::vector< std::vector<double>> vvdTranspose(const std::vector< std::vector<double>>& vvdTable);
	//vvi表转置
	std::vector< std::vector<int>> vviTranspose(const std::vector< std::vector<int>>& vviTable);

	//输入点坐标数组1 2   均为n*3的表，输出变换阵
	//vvdPoints3DX X点 n*3
	//vvdPoints3DY Y点 n*3
	//获取两组3D点间的仿射变换矩阵 支持4组点或更多组点 即支持最小二层求解
	//std::vector< std::vector<double>> PoseX = ReadTxtTableDouble("TestData/AffineTransform/PoseX.txt", " ");
	//std::vector< std::vector<double>> PoseY = ReadTxtTableDouble("TestData/AffineTransform/PoseY.txt", " ");
	//cv::Mat HomMat4d = getAffineTransform3D(PoseX, PoseY);
	//SaveMatToXML(HomMat4d, "TestData/AffineTransform/HomMat3DCamInBase.xml", "HomMat3DCamInBase");
	cv::Mat getAffineTransform3D(const std::vector< std::vector<double>>& vvdPoints3DX, const std::vector< std::vector<double>>& vvdPoints3DY);

	//对cvCloud点云进行仿射变换
	void AffineTransform3D(const cv::Mat& cvCloudIn, const cv::Mat& matAffine, cv::Mat* pcvCloudOut);

	//变换3D点坐标
	cv::Point3d AffineTransPonit3d(const cv::Point3d& PointX, const cv::Mat& C1InC2);
	//cv::Point3d AffineTransPonit3d(const cv::Point3d& PointX, const Eigen::Matrix4d& C1InC2);

	//位姿矩阵变换：Matrix4d转Matrix4f
	Eigen::Matrix4f Matrix4d2Matrix4f(Eigen::Matrix4d dMatrix);
	//位姿矩阵变换：Matrix4f转Matrix4d
	Eigen::Matrix4d Matrix4f2Matrix4d(Eigen::Matrix4f fMatrix);

	//欧拉角转换为3*3旋转矩阵位姿
	void Euler2RotationMatrix(double dXA, double dYA, double dZA, Eigen::Matrix3d* pdRotationMatrix);
	Eigen::Matrix3d Euler2RotationMatrix(double dXA, double dYA, double dZA);

	//欧拉角位姿转换为4*4矩阵位姿
	void EulerPose2MatrixPose(double dX, double dY, double dZ, double dXA, double dYA, double dZA, Eigen::Matrix4d* pdMatrixPose);
	Eigen::Matrix4d EulerPose2MatrixPose(double dX, double dY, double dZ, double dXA, double dYA, double dZA);

	//4*4矩阵位姿转欧拉角位姿
	void MatrixPose2EulerPose(Eigen::Matrix4f fMatrixPose, double* pdX, double* pdY, double* pdZ, double* pdXA, double* pdYA, double* pdZA);
	//4*4矩阵位姿转欧拉角位姿（未测试）
	void MatrixPose2EulerPose(Eigen::Matrix4d dMatrixPose, double* pdX, double* pdY, double* pdZ, double* pdXA, double* pdYA, double* pdZA);

	//将欧拉角等效转换为另一种表示
	void eulerAngleTransverter(double dxA, double dyA, double dzA, double* pdxA2, double* pdyA2, double* pdzA2);

	//将欧拉角等效转换为绝对值近零的角度 //转换欧拉角为近零的一组角度
	void eulerAngleTranMini(double dxA, double dyA, double dzA, double* pdxA2, double* pdyA2, double* pdzA2);

	//欧拉角转换为四元数 待测试使用
	void eulerAngle2quaternion(double dxA, double dyA, double dzA, double* pdw, double* pdx, double* pdy, double* pdz);
	//欧拉角位姿转换为四元数位姿 v6 ->v7 待测试使用
	std::vector<float>  eulerPose2quaternionPose(std::vector<float> PoseEuler);

	//欧拉角转换为四元数 xyz欧拉角顺序
	void eulerAngle2quaternion2(double dxA, double dyA, double dzA, double* pdw, double* pdx, double* pdy, double* pdz);
	//欧拉角位姿转换为四元数位姿 v6 ->v7 xyz欧拉角顺序
	std::vector<float>  eulerPose2quaternionPose2(std::vector<float> PoseEuler);
};
